perm filename TRAJ.SAI[SYS,HE]12 blob
sn#098476 filedate 1974-04-25 generic text, type T, neo UTF8
00100 DEFINE WRITE="DEB_HAND";
00200 EXTERNAL INTEGER _SKIP_;
00300 DEFINE AVT (A, S, CA, SA)="1.0, -CA, SA, A, 1.0, CA, -SA, A, 0.0, SA, CA, S, [3] 0.0, 1.0, 0.0 ";
00400 DEFINE AVP (A, S, CA, SA)="-CA, SA, A, CA, -SA, A";
00500 DEFINE AVS (A, CA, SA, CT, ST)="CT, -CA*ST, SA*ST, A*CT, ST, CA*CT, -SA*CT, A*ST,
00600 0.0, SA, CA, 1.0, [3] 0.0, 1.0, 0.0 ";
00700 DEFINE QT = "0.0, -1.0, 0.0, 0.0, 1.0, [12] 0.0 ";
00800 DEFINE QS = "[11] 0.0, 1.0, [5] 0.0 ";
00900 DEFINE JDEF (M, X, Y, Z, IXX, IYY, IZZ) =".5*(-IXX+IYY+IZZ), 0.0, 0.0, M*X, 0.0,
01000 .5*(IXX-IYY+IZZ), 0.0, M*Y, 0.0, 0.0, .5*(IXX+IYY-IZZ), M*Z, M*X, M*Y, M*Z, M, 0.0 ";
01100 IFC BLUE THENC
01200 REQUIRE "BLUE.DAT" SOURCE_FILE;
01300 ELSEC
01400 REQUIRE "YELLOW.DAT" SOURCE_FILE;
01500 ENDC
01600 SAFE REAL ARRAY U1,T[0:101];
01700 SAFE REAL ARRAY DIAG[1:7];
01800 SAFE REAL ARRAY LAST_PLANNED_TRANS,PARK_TRANS[1:4,1:4];
01900 SAFE REAL ARRAY LAST_PLANNED_ARM[1:6];
02000 SAFE REAL ARRAY ARRIVE_ARM,DEPART_ARM[1:4];
02100 REAL F1_ARM,F2_ARM,OBJECT_MASS,OBJECT_KXX,OBJECT_KYY,OBJECT_KZZ;
02200 INTEGER T1_ARM,T2_ARM,MERGE;
02300 INTEGER BFJ;
02400 DEFINE MAX_SEG="8";
02500 SAFE REAL ARRAY POSITION[1:6,0:MAX_SEG];
02600 SAFE REAL ARRAY KOE[1:6,1:(MAX_SEG*3)+6];
02700 SAFE INTEGER ARRAY PERIOD[1:6,1:MAX_SEG];
02800 SAFE INTEGER ARRAY NS[1:6];
02900 SAFE REAL ARRAY POS[0:MAX_SEG];
03000 SAFE REAL ARRAY SOLN[1:(MAX_SEG*3)+6];
03100 SAFE INTEGER ARRAY TIM[1:MAX_SEG];
03200 SAFE STRING ARRAY BANDS[0:'37];
03300 SAFE INTEGER ARRAY TRACK[0:'40];
03400 INTEGER NEXT_BAND,BAND,BAND_NUMBER;
03500 PRELOAD_WITH 0,'1040,0;
03600 SAFE INTEGER ARRAY GOODIE[1:3];
03700 DEFINE UFBWRT="'707000000000";
03800 BOOLEAN FORCED,WAS_FORCED,FAST;
03900 SAFE REAL ARRAY LAST_ARM[1:6];
04000 SAFE REAL ARRAY LAST_TRANS[1:4,1:4];
04100 SAFE REAL ARRAY QA,D[0:16];
04200 SAFE REAL ARRAY TF[1:6];
04300 SAFE REAL ARRAY LU,NR[1:6,1:6];
04400 REAL DIGITS;
04500 INTEGER LOOP,JOINT,DURATION;
04600 SAFE REAL ARRAY REAL_DATA[1:18];
04700 REQUIRE "MATRIX.REL[SYS,HE]" LOAD_MODULE;
04800 REQUIRE "SAILIB.REL[SYS,HE]" LOAD_MODULE;
04900 EXTERNAL SIMPLE PROCEDURE MMOVE(REFERENCE REAL R,A);
05000 EXTERNAL SIMPLE PROCEDURE MTIMES(REFERENCE REAL R,A,B);
05100 EXTERNAL REAL SIMPLE PROCEDURE TRACET(REFERENCE REAL X,Y);
05200 EXTERNAL SIMPLE PROCEDURE DECOMPOSE(INTEGER I; REAL ARRAY NR,LU);
05300 EXTERNAL SIMPLE PROCEDURE SOLVE(INTEGER I;REAL ARRAY NR,LU,DTH);
05400 EXTERNAL SIMPLE PROCEDURE IMPROVE(INTEGER I; REAL ARRAY NR,LU,DA,DTH;REFERENCE REAL DIGITS);
05500 INTEGER SIMPLE PROCEDURE SQAR (INTEGER I);
05600 RETURN ((I-1)*17);
05700
00100 EXTERNAL INTEGER SIMPLE PROCEDURE PSTRING(REFERENCE INTEGER I;STRING S);
00200
00300 SIMPLE PROCEDURE COMPLETA (INTEGER I;SAFE REAL ARRAY TH);
00400 BEGIN REAL S,C;
00500 INTEGER J,K;
00600 J←SQAR(I);
00700 K←6*(I-1);
00800 S←SIND(TH[I]);
00900 C←COSD(TH[I]);
01000 A[J]←C;
01100 A[J+1]←APAR[K]*S;
01200 A[J+2]←APAR[K+1]*S;
01300 A[J+3]←APAR[K+2]*C;
01400 A[J+4]←S;
01500 A[J+5]←APAR[K+3]*C;
01600 A[J+6]←APAR[K+4]*C;
01700 A[J+7]←APAR[K+5]*S;
01800 END;
01900
02000 SIMPLE PROCEDURE HANDPOS(SAFE REAL ARRAY TH);
02100 BEGIN INTEGER I;
02200 FOR I←1,2,4,5,6 DO COMPLETA(I,TH);
02300 A[SQAR(3)+11]←TH[3];
02400 A[3]←A[3]+SHOLDER[1];
02500 A[7]←A[7]+SHOLDER[2];
02600 MMOVE(A[0],A[0]);
02700 MMOVE(T[0],A[0]);
02800 UNDERFLOW(14);
02900 FOR I←2 STEP 1 UNTIL 6 DO MTIMES(T[SQAR(I)],T[SQAR(I-1)],A[SQAR(I)]);
03000 UNDERFLOW(0);
03100 END;
03200
03300 SIMPLE PROCEDURE UPDATE_SEG;
03400 BEGIN INTEGER I;
03500 HANDPOS(ARM_VECTOR);
03600 FOR I←3 STEP 1 UNTIL 6 DO
03700 BEGIN
03800 ARRBLT(ARM_LINK[I,1,1],T[SQAR(I)],16);
03900 END;
04000 GRASP←ARM_VECTOR[7];
04100 END;
04200
04300 SIMPLE PROCEDURE NOT_LESS(REFERENCE REAL V;REAL L);
04400 BEGIN
04500 IF V<0.0 ∧ V>-L THEN BEGIN V←0; RETURN END;
04600 IF V≥0.0 ∧ V< L THEN V←0;
04700 END;
04800
00100 SIMPLE PROCEDURE INCREMENT(SAFE REAL ARRAY DTH,DX_DY_DZ;BOOLEAN COMP);
00200 BEGIN PRELOAD_WITH 3,7,11;
00300 SAFE OWN INTEGER ARRAY KEY[1:6];
00400 PRELOAD_WITH [6] 0.0; SAFE OWN REAL ARRAY DIR[1:6];
00500 INTEGER I,J;
00600 REAL MAX,R;
00700 LABEL OUT;
00800 REDUCE(DX_DY_DZ);
00900 IF DX_DY_DZ[1]=0.0 ∧
01000 DX_DY_DZ[2]=0.0 ∧
01100 DX_DY_DZ[3]=0.0 THEN BEGIN FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←0.0;
01200 RETURN;
01300 END;
01400 UNDERFLOW(14);
01500 ARRBLT(DIR[1],DX_DY_DZ[1],3);
01600 IF ¬COMP THEN BEGIN
01700 IF ABS(T[85+2])<ABS(T[85+6])
01800 THEN BEGIN KEY[4]←2;
01900 KEY[5]←IF ABS(T[85+6])<ABS(T[85+10]) THEN 6 ELSE 10
02000 END
02100 ELSE BEGIN KEY[4]←6;
02200 KEY[5]←IF ABS(T[85+2])<ABS(T[85+10]) THEN 2 ELSE 10
02300 END;
02400 KEY[6]←IF ABS(T[85+4])>ABS(T[85+0])
02500 THEN IF ABS(T[85+8])>ABS(T[85+4]) THEN 9 ELSE 5
02600 ELSE IF ABS(T[85+8])>ABS(T[85+0]) THEN 9 ELSE 1;
02700 MMOVE(U1[SQAR(6)],A[SQAR(6)]);
02800 FOR I←5 STEP -1 UNTIL 2 DO
02900 BEGIN MTIMES(U1[SQAR(I)],A[SQAR(I)],U1[SQAR(I+1)]);
03000 MTIMES(U1[SQAR(I+1)],Q[(IF I=2 THEN 17 ELSE 0)],U1[SQAR(I+1)])
03100 END;
03200 A[3]←A[7]←0.0;
03300 MMOVE(A[0],A[0]);
03400 MTIMES(U1[0],A[0],U1[SQAR(2)]);
03500 MTIMES(U1[SQAR(2)],Q[0],U1[SQAR(2)]);
03600 FOR I←2 STEP 1 UNTIL 6 DO MTIMES(U1[SQAR(I)],T[SQAR(I-1)],U1[SQAR(I)]);
03700 MTIMES(U1[0],Q[0],U1[0]);
03800 FOR I←1 STEP 1 UNTIL 6 DO
03900 FOR J←1 STEP 1 UNTIL 6 DO
04000 NR[J,I]←U1[SQAR(I)+KEY[J]];
04100 DECOMPOSE(6,NR,LU);
04200 END;
04300 SOLVE(6,LU,DIR,DTH);
04400 IMPROVE(6,NR,LU,DIR,DTH,DIGITS);
04500 MAX←0;
04600 FOR I←1 STEP 1 UNTIL 6 DO IF (R←ABS(DIR[I]))>MAX THEN MAX←R;
04700 FOR I←1 STEP 1 UNTIL 6 DO IF (R←ABS(DTH[I]))>MAX THEN
04800 BEGIN FOR J←I+1 STEP 1 UNTIL 6 DO IF ABS(DTH[I]+DTH[J])/R<0.5 THEN
04900 BEGIN DTH[I]←0;
05000 DTH[J]←0;
05100 GO TO OUT;
05200 END;
05300 END;
05400 OUT: FOR I←1 STEP 1 UNTIL 6 DO BEGIN
05500 DTH[I]←IF I=3 THEN DTH[I] ELSE RAD*DTH[I];
05600 NOT_LESS(DTH[I],@-5);
05700 END;
05800 UNDERFLOW(0);
05900 END;
06000
06100 INTEGER SIMPLE PROCEDURE LIMITS(SAFE REAL ARRAY JOINT);
06200 BEGIN INTEGER I;
06300 FOR I←1 STEP 1 UNTIL 6 DO
06400 IF (STOP[I,1]-JOINT[I])*(JOINT[I]-STOP[I,2])<0.0 THEN RETURN(I);
06500 RETURN (0);
06600 END;
06700
06800 SIMPLE PROCEDURE FAST_WRITE;
06900 IF FAST
07000 THEN BEGIN LABEL OK;
07100 START_CODE LABEL WRT;
07200 HRRZ 1,GOODIE;
07300 HRRZ 2,COEFF;
07400 MOVEM 2,(1);
07500 MOVE 3,BAND_NUMBER;
07600 MOVEI 4,10;
07700 WRT: UFBWRT 3,(1);
07800 SOJGE 4,WRT;
07900 MOVEI 2,'20;
08000 ADDM 2,2(1);
08100 JUMPGE 4,OK;
08200 END;
08300 USERERR(0,1,"TRAJECTORY WRITE ERROR");
08400 OK: END
08500 ELSE ARRYOUT('16,COEFF[0],'1000);
08600
00100 SIMPLE PROCEDURE FORCE(SAFE REAL ARRAY TQ,FV);
00200 BEGIN
00300 STRING S;
00400 INTEGER I,L;
00500 REAL XS,YS;
00600 SAFE OWN REAL ARRAY F,M,P[1:4];
00700 SAFE OWN REAL ARRAY INV[1:4,1:4];
00800 EXTERNAL SIMPLE PROCEDURE TRANSFORM(REAL ARRAY R;REFERENCE REAL A;REAL ARRAY V);
00900 EXTERNAL SIMPLE PROCEDURE INVERT(REFERENCE REAL R,A);
01000 SIMPLE PROCEDURE COLVECT(REAL ARRAY R;INTEGER L,I);
01100 BEGIN INTEGER K;
01200 FOR K←1 STEP 1 UNTIL 3 DO R[K]←A[L+(K-1)*4+(I-1)];
01300 R[4]←1.0;
01400 END;
01500
01600 PUSH_FORMAT(9,1);
01700 ARRBLT(F[1],FV[1],3);
01800 ARRBLT(M[1],FV[4],3);
01900 INVERT(INV[1,1],T[SQAR(6)]);
02000 F[4]←M[4]←0.0;
02100 TRANSFORM(F,INV[1,1],F);
02200 TRANSFORM(M,INV[1,1],M);
02300 A[3]←A[7]←0.0;
02400 FOR L←6 STEP -1 UNTIL 1 DO
02500 BEGIN
02600 F[4]←M[4]←0.0;
02700 TRANSFORM (F,A[SQAR(L)],F);
02800 COLVECT(P,SQAR(L),4);
02900 F[4]←1.0;
03000 CROSS(P,P,F);
03100 TRANSFORM(M,A[SQAR(L)],M);
03200 M[4]←1.0;
03300 PLUS(M,M,P);
03400 REDUCE(M);
03500 TQ[L]←IF L=3 THEN F[3] ELSE M[3];
03600 END;
03700 A[3]←A[3]+SHOLDER[1];
03800 A[7]←A[7]+SHOLDER[2];
03900 S←"JOINT TORQUES";
04000 FOR I←1 STEP 1 UNTIL 6 DO S←S&(CVF(TQ[I]));
04100 IF TYP_HAND THEN OUTSTR(S&CRLF&CRLF);
04200 POP_FORMAT;
04300 END;
04400
04500
04600 PRELOAD_WITH
04700 '400000200000,
04800 '200000040000,
04900 '100000010000,
05000 '40000002000,
05100 '20000000400,
05200 '10000000100;
05300 SAFE OWN INTEGER ARRAY GO_WORD[1:6];
05400 SIMPLE PROCEDURE BITS(REFERENCE INTEGER DATWD;REAL SIGN;REFERENCE INTEGER DAT);
05500 START_CODE
05600 MOVE 3,-3('17);
05700 MOVE 1,(3);
05800 MOVE 2,@-1('17);
05900 TDO 1,2;
06000 TRZ 1,(2);
06100 LSH 2,1;
06200 SKIPGE -2('17);
06300 TRO 1,(2);
06400 MOVEM 1,(3);
06500 END;
06600
06700 PROCEDURE FREE_JOINT(REFERENCE INTEGER DATWD);
06800 BEGIN INTEGER NF,I,J,K;
06900 REAL R,MAX;
07000 SAFE OWN INTEGER ARRAY ZF[1:6];
07100 SAFE OWN REAL ARRAY FV,TQ[1:6];
07200 NF←FREE_ARM[0,1];
07300 FOR I←1 STEP 1 UNTIL NF DO
07400 BEGIN
07500 ARRBLT(FV[1],FREE_ARM[I,1],6);
07600 FORCE(TQ,FV);
07700 MAX←0.0;
07800 FOR J←1 STEP 1 UNTIL 6 DO
07900 BEGIN LABEL L1;
08000 IF J=BFJ THEN GO TO L1;
08100 FOR K←1 STEP 1 UNTIL I-1 DO IF ZF[K]=J THEN GO TO L1;
08200 IF (R←ABS(TQ[J]/F0[J]))>MAX THEN
08300 BEGIN
08400 MAX←R;
08500 ZF[I]←J;
08600 END;
08700 L1: END;
08800 BITS(DATWD,TQ[ZF[I]],GO_WORD[ZF[I]]);
08900 END;
09000 END;
09100
09200 SIMPLE INTEGER PROCEDURE RUNTIME(SAFE REAL ARRAY DTH);
09300 BEGIN
09400 INTEGER T,TIME,I;
09500 TIME←0;
09600 FOR I←1 STEP 1 UNTIL 6 DO BEGIN
09700 T←ABS(DTH[I])*TIMFAC[I];
09800 IF T>TIME THEN TIME←T END;
09900 RETURN(TIME+20);
10000 END "RUNTIME";
10100
00100 SIMPLE PROCEDURE SCHEINMAN (SAFE REAL ARRAY RES,TH);
00200 BEGIN
00300 LABEL F2;
00400 REAL M,IXX,IYY,IZZ;
00500 SAFE OWN REAL ARRAY JI,TQ[1:6];
00600 EXTERNAL REAL SIMPLE PROCEDURE INNER(REFERENCE REAL A,B);
00700 REAL Z; INTEGER I,DIFF,J,K,L,TJ;
00800 RES[1]←0;ARRBLT(RES[2],RES[1],6);
00900 JI[1]←0;ARRBLT(JI[2],JI[1],5);
01000 WAS_FORCED←FORCED;
01100 FORCED←FALSE;
01200 FORCED←OBJECT_MASS;
01300 M←9.33*OBJECT_MASS+M6;
01400 Z←(M6*Z6)/M;
01500 IXX←I6XX+9.33*OBJECT_MASS*OBJECT_KXX↑2;
01600 IYY←I6YY+9.33*OBJECT_MASS*OBJECT_KYY↑2;
01700 IZZ←I6ZZ+9.33*OBJECT_MASS*OBJECT_KZZ↑2;
01800 I←SQAR(6);
01900 JMAT[I]←0.5*(-IXX+IYY+IZZ);
02000 JMAT[I+5]←0.5*(IXX-IYY+IZZ);
02100 JMAT[I+10]←0.5*(IXX+IYY-IZZ);
02200 JMAT[I+11]←JMAT[I+14]←M*Z;
02300 JMAT[I+15]←M;
02400 COMPLETA(1,TH);
02500 COMPLETA(2,TH);
02600 A[SQAR(3)+11]←TH[3];
02700 COMPLETA(4,TH);
02800 COMPLETA(5,TH);
02900 COMPLETA(6,TH);
03000 UNDERFLOW(14);
03100 MTIMES(U1[0],Q[0],A[0]);
03200 MTIMES(D[0],U1[0],JMAT[0]);
03300 JI[1]←TRACET(D[0],U1[0]);
03400 MMOVE(T[0],A[0]);
03500 for J ← 2 step 1 until 6 do begin
03600 TJ←SQAR(J);
03700 DIFF←IF J=3 THEN 17 ELSE 0;
03800 MTIMES (QA[0], Q[DIFF], A[TJ]);
03900 MTIMES (U1[TJ], T[SQAR(J-1)], QA[0]);
04000 for I← 1 step 1 until J-1 do MTIMES (U1[SQAR(I)],U1[SQAR(I)], A[TJ]);
04100 MTIMES(T[TJ],T[SQAR(J-1)],A[TJ]);
04200 F2: FOR I←1 STEP 1 UNTIL J DO RES[I]←RES[I]
04300 +0.107*INNER(U1[SQAR(I)+8],JMAT[TJ+12]);
04400 For K← 1 step 1 until J do begin
04500 MTIMES (D[0],U1[SQAR(K)],JMAT[TJ]);
04600 JI[K]←JI[K] + TRACET(D[0],U1[SQAR(K)]);
04700 END;
04800 END;
04900 T[88]←T[88]+SX;T[92]←T[92]+SY;
05000 FOR I←1 STEP 1 UNTIL 6 DO IF FORCE_ARM[I] THEN BEGIN
05100 FORCE(TQ,FORCE_ARM);
05200 FORCED←TRUE;
05300 FOR I←1 STEP 1 UNTIL 6 DO RES[I]←RES[I]+TQ[I];
05400 DONE;
05500 END;
05600 FOR I←1 STEP 1 UNTIL 6 DO NOT_LESS(RES[I],1.0@-5);
05700 IFC BLUE THENC
05800 FOR I←1 STEP 1 UNTIL 6 DO RES[I]←(RES[I] LAND '777777000000) LOR (SQRT(JI[I]) LSH -18);
05900 I←'770000252504;
06000 ELSEC
06100 FOR I←1 STEP 1 UNTIL 6 DO RES[I]←(RES[I] LAND '777777000000) LOR (SQRT(JI[I]) LSH -18);
06200 I←'770000252502;
06300 ENDC
06400 IF NNUL THEN I←I+'1000000;
06500 IF ¬(FORCED ∨ WAS_FORCED) THEN I←I+'2000000;
06600 FREE_JOINT(I);
06700 ARRBLT(RES[7],I,1);
06800 UNDERFLOW(0);
06900 END;
00100 SIMPLE PROCEDURE FLUSH(BOOLEAN FINISH;SAFE REAL ARRAY TS);
00200 BEGIN INTEGER I;
00300 IF ¬FRST_OPEN ∨ FINISH
00400 THEN BEGIN
00500 FOR I←1 STEP 1 UNTIL FREEL
00600 DO FOR J←1 STEP 1 UNTIL PTR3+1
00700 DO IF EQU(REF[J],LABELS[I])
00800 THEN BEGIN
00900 IF (STACK[J] LAND '77000000 ) = '26000000 THEN BEGIN
01000 N←PTRS[I]-J+COEFF[(STACK[J] LAND '777777)+1];
01100 REF[J]←NULL;
01200 IF N+J<1 ∨ N+J>PTR3+1
01300 THEN BEGIN
01400 OUTSTR(LABEL_LINE[I]&LABELS[I]&" OUT OF RANGE"&'15&'12);
01500 N←PTR3+1-J END;
01600 COEFF[(STACK[J] LAND '777777) +1]←N;
01700 LABEL_LINE[I]←NULL END ELSE BEGIN
01800 START_CODE
01900 MOVE 1,STACK;
02000 ADD 1,J;
02100 HRRE 1,-1(1);
02200 MOVEM 1,N END;
02300 N←PTRS[I]-J+N;
02400 REF[J]←NULL;
02500 IF N+J<1 ∨ N+J>PTR3+1
02600 THEN BEGIN
02700 OUTSTR(LABEL_LINE[I]&LABELS[I]&" OUT OF RANGE"&'15&'12);
02800 N←PTR3+1-J END;
02900 STACK[J]←(N LAND '777777) LOR (STACK[J] LAND '777000000);
03000 LABEL_LINE[I]←NULL END;END;
03100 FOR I←1 STEP 1 UNTIL PTR3
03200 DO IF LENGTH(REF[I])
03300 THEN BEGIN OUTSTR(CODE_LINE[I]&REF[I]&" UNDEFINED"&'15&'12);
03400 STACK[I]←(PTR3+1-I) LOR '102000000;
03500 REF[I]←NULL;
03600 LABEL_LINE[I]←NULL END;
03700 FREEL←0;
03800 FOR I←1 STEP 1 UNTIL PTR3 DO
03900 IF (J←(STACK[I] LAND '777000000))='102000000 ∨ J='16000000 THEN BEGIN
04000 START_CODE
04100 MOVE 1,STACK;
04200 ADD 1,I;
04300 HRRE 1,-1(1);
04400 MOVEM 1,N END;
04500 IF ¬N ∨ N+I<1 ∨ N+I>PTR3+1 THEN BEGIN
04600 OUTSTR(CODE_LINE[I]&"JUMP OUT OF RANGE"&CRLF);
04700 STACK[I]←(PTR3+1-I) LOR '102000000 END;
04800 END;
04900 FOR I←1 STEP 1 UNTIL MAC DO BBEG[I]←LLAB[I]←1;
05000 IF PTR3+PTR4≥PTR2 THEN USERERR(0,1,"TRAJECTORY FILE TOO LONG");
05100 ARRBLT(COEFF[PTR4+1],STACK[1],PTR3);
05200 COEFF[PTR4+PTR3+1]←0;
05300 I←-(PTR4+1+(MASTER LSH 18));
05400 ARRBLT(COEFF[0],I,1);
05500 COEFF['1000]←0;
05600 PTR4←PTR3←0;
05700 PTR2←512;
05800 FAST_WRITE;
05900 END ELSE FRST_OPEN←FALSE;
06000 IF FINISH THEN BEGIN
06100 IF ¬FAST THEN RELEASE('16);
06200 RETURN END;
06300 ARRTRAN(TH,TS);
06400 SCHEINMAN(DIAG,TH);
06500 END;
06600
06700 SIMPLE PROCEDURE FLUSHP(INTEGER N;SAFE REAL ARRAY TH);
06800 BEGIN FRST_OPEN←FALSE;
06900 IF PTR3+PTR4+N≥PTR2 THEN FLUSH(0,TH);
07000 END;
07100
00100 SIMPLE PROCEDURE REVOLVE(SAFE REAL ARRAY P,O; REAL TH);
00200 BEGIN
00300 SAFE OWN REAL ARRAY OP,A,T[1:4];
00400 UNIT(O,O);
00500 SCALE(OP,O,DOT(P,O));
00600 DIFFERENCE(A,P,OP);
00700 CROSS(T,O,A);
00800 SCALE(T,T,SIND(TH));
00900 SCALE(P,A,COSD(TH));
01000 PLUS(P,P,T);
01100 PLUS(P,P,OP);
01200 REDUCE(P);
01300 END;
01400 PROCEDURE RESET_CONO;
01500 BEGIN
01600 FREE_ARM[0,1]←FORCE_ARM[1]←OBJECT_MASS←OBJECT_KXX←OBJECT_KYY←OBJECT_KZZ←0;
01700 NNUL←0;
01800 BFJ←0;
01900 ARRBLT(FORCE_ARM[2],FORCE_ARM[1],5);
02000 END;
02100
02200
02300 SIMPLE PROCEDURE LIFTOFF(SAFE REAL ARRAY TH,DTH,DIR;REFERENCE INTEGER N,T1);
02400 BEGIN INTEGER I,J;
02500 REAL F;
02600 LABEL L2;
02700 SAFE OWN REAL ARRAY TT[1:6];
02800 F←1.0;
02900 IF LIMITS(TH) THEN BEGIN
03000 FOR I←1 STEP 1 UNTIL 5 DO DTH[I]←
03100 IF TH[I]≥STOP[I,2] THEN STOP[I,2]-TH[I]
03200 ELSE IF TH[I]≤STOP[I,1] THEN STOP[I,1]-TH[I]
03300 ELSE 0;
03400 GO TO L2;
03500 END;
03600 HANDPOS(TH);
03700 INCREMENT(DTH,DIR,FALSE);
03800 FOR I←1 STEP 1 UNTIL 6 DO TT[I]←TH[I]+DTH[I];
03900 WHILE I←LIMITS(TT) DO
04000 BEGIN FOR J←1,2 DO
04100 IF (TT[I]-STOP[I,J])*(STOP[I,J]-TH[I])≥0
04200 THEN
04300 BEGIN F←0.4*(STOP[I,J]-TH[I])/(TT[I]-TH[I]);
04400 DTH[I]←DTH[I]*F;
04500 TT[I]←TH[I]+DTH[I];
04600 DONE;
04700 END;
04800 END;
04900 L2: N←IF T1>0 THEN T1 ELSE RUNTIME(DTH);
05000 END;
05100
00100 EXTERNAL SIMPLE PROCEDURE PACK(REFERENCE INTEGER PTR;INTEGER PERIOD;REFERENCE REAL BUF);
00200 SIMPLE PROCEDURE PACK_UP;BEGIN
00300 INTEGER P1,I,J,K;
00400 REAL R1,R2;
00500 SAFE OWN INTEGER ARRAY LOOPP,NXT,ALT[1:6];
00600 SAFE OWN REAL ARRAY BUF[0:4];
00700 COEFF[PTR4←PTR4+1]←DURATION;
00800 STACK[PTR3←PTR3+1]←'21000000 LOR PTR4;
00900 IF MERGE THEN BEGIN
01000 STACK[PTR3]←STACK[PTR3] LOR '400000000;
01100 STACK[PTR3]↔STACK[PTR3-1];
01200 MERGE←FALSE END;
01300 BUF[1]←0;BUF[2]←0;
01400 FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
01500 BUF[0]←POSITION[JOINT,0];
01600 BUF[3]←KOE[JOINT,1];BUF[4]←KOE[JOINT,2];
01700 PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,1],BUF[0]);
01800 NXT[JOINT]←PTR4 END;
01900 COEFF[PTR4←PTR4+1]←PTR2;
02000 FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
02100 BUF[4]←0;
02200 K←IF LOOP THEN 7 ELSE NS[JOINT]-1;
02300 FOR I←2 STEP 1 UNTIL K DO BEGIN
02400 BUF[0]←POSITION[JOINT,I-1];
02500 FOR J←1 STEP 1 UNTIL 3 DO BUF[J]←KOE[JOINT,(3*I)-4+J];
02600 PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,I],BUF[0]);
02700 COEFF[NXT[JOINT]]←(PTR4 LSH 18) LOR COEFF[NXT[JOINT]];
02800 NXT[JOINT]←PTR4;
02900 IF LOOP THEN BEGIN
03000 IF I=3 THEN LOOPP[JOINT]←PTR4;
03100 IF I=6 THEN ALT[JOINT]←PTR4 END;
03200 IF JOINT=6 THEN COEFF[PTR4←PTR4+1]←PTR2←PTR2-7 END;
03300 BUF[0]←POSITION[JOINT,I-1];
03400 BUF[1]←-4.0*(R1←KOE[JOINT,(3*I)-2])+3.0*(R2←KOE[JOINT,(3*I)-3]);
03500 BUF[2]← 6.0*R1-3.0*R2;
03600 BUF[3]←-4.0*R1 +R2;
03700 BUF[4]← R1;
03800 PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,I],BUF[0]);
03900 COEFF[NXT[JOINT]]←(PTR4 LSH 18) LOR COEFF[NXT[JOINT]];
04000 BUF[4]←0;
04100 IF JOINT=6 THEN COEFF[PTR4←PTR4+1]←PTR2←PTR2-7;
04200 IF LOOP THEN BEGIN
04300 BUF[0]←POSITION[JOINT,6];
04400 FOR J←1 STEP 1 UNTIL 4 DO BUF[J]←KOE[JOINT,3*I-2+J];
04500 PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,3],BUF[0]);
04600 NXT[JOINT]←PTR4;
04700 COEFF[P1←ALT[JOINT]]←(PTR4 LSH 27) LOR COEFF[P1];
04800 IF JOINT=6 THEN COEFF[P1+1]←'100000000000 LOR (LOOP LSH 18) LOR COEFF[P1+1];
04900 IF JOINT=6 THEN COEFF[PTR4←PTR4+1]←499;
05000 BUF[0]←POSITION[JOINT,0];
05100 FOR J←1 STEP 1 UNTIL 4 DO BUF[J]←KOE[JOINT,3*I+2+J];
05200 PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,3],BUF[0]);
05300 COEFF[NXT[JOINT]]←(PTR4 LSH 18) LOR COEFF[NXT[JOINT]];
05400 COEFF[PTR4]←(LOOPP[JOINT] LSH 18) LOR COEFF[PTR4];
05500 IF JOINT=6 THEN COEFF[PTR4←PTR4+1]←473 END END;
05600 END;
05700
05800 INTEGER SIMPLE PROCEDURE QUADROOT(REAL A0,A1,A2,A3;REFERENCE REAL ANG,T);BEGIN
05900 REAL DISC,B;
06000 INTEGER I;
06100 IF A3 ∧ (DISC←(A2/(3*A3))↑2-A1/(3*A3))≥0 THEN DISC←SQRT(DISC) ELSE RETURN(0);
06200 B←A2/(3*A3);
06300 T←DISC-B;
06400 FOR I←0,1 DO BEGIN
06500 IF 0< T <1.0 THEN
06600 BEGIN
06700 ANG←(((A3*T)+A2)*T+A1)*T+A0;
06800 IF ANG>STOP[JOINT,2] THEN BEGIN ANG←STOP[JOINT,2];RETURN(1) END;
06900 IF ANG<STOP[JOINT,1] THEN BEGIN ANG←STOP[JOINT,1];RETURN(1) END;
07000 END;
07100 T←-(DISC+B);
07200 END;
07300 RETURN(0);
07400 END;
07500
00100 IFC WAVE THENC
00200 SIMPLE PROCEDURE STEP_ARM(INTEGER JOINT;REAL DTH;INTEGER TIME);
00300 BEGIN
00400 INTEGER I;
00500 IF ARM_EXECUTE THEN RETURN;
00600 FLUSHP(100,LAST_ARM);
00700 FOR I←1 STEP 1 UNTIL 6 DO
00800 BEGIN
00900 POSITION[I,0]←LAST_ARM[I];
01000 NS[I]←2;
01100 PERIOD[I,1]←10;
01200 PERIOD[I,2]←IF TIME>60 THEN TIME-10 ELSE 50;
01300 IF I=JOINT THEN POSITION[I,1]←POSITION[I,2]←LAST_ARM[I]←LAST_ARM[I]+DTH
01400 ELSE POSITION[I,1]←POSITION[I,2]←LAST_ARM[I];
01500 END;
01600 SCHEINMAN(DIAG,LAST_ARM);
01700 ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
01800 IFC BLUE THENC
01900 COEFF[PTR2+6]←GO_WORD[JOINT]+2;
02000 ELSEC
02100 COEFF[PTR2+6]←GO_WORD[JOINT]+2;
02200 ENDC
02300 IF NNUL THEN COEFF[PTR2+6]←COEFF[PTR2+6]+'1000000;
02400 ARRBLT(COEFF[PTR2-7],COEFF[PTR2],7);
02500 KOE[1,1]←0;
02600 ARRBLT(KOE[1,2],KOE[1,1],6*((MAX_SEG*3)+6)-1);
02700 PACK_UP;
02800 DURATION←10;
02900 END;
03000
03100 ELSEC
03200 SIMPLE MESSAGE PROCEDURE SET_TRANS(STRING S;REAL ARRAY T);
03300 BEGIN "SET_TRANS"
03400 INTEGER PTR;
03500 STRING SS;
03600 SAFE OWN REAL ARRAY E[1:6];
03700 UNSTRUCT(T,E);
03800 PTR←HASH(S);
03900 WHILE LENGTH(SS←TRANSNAM[PTR]) ∧ ¬EQU(SS,S) DO PTR←REHASH;
04000 IF ¬LENGTH(SS)
04100 THEN BEGIN
04200 IF FREE_DATA+2>FREE_DATA_LENGTH THEN RETURN;
04300 TRANSNAM[PTR]←S;
04400 TRANSNUM[PTR]←FREE_DATA;
04500 FREE_DATA←FREE_DATA+2 END;
04600 ARRBLT(DATA_BASE[TRANSNUM[PTR],1],E[1],6)
04700 END "SET_TRANS";
04800
04900 SIMPLE MESSAGE PROCEDURE SET_VECT(STRING S;REAL ARRAY V);
05000 BEGIN "SET_VECT"
05100 INTEGER PTR;
05200 STRING SS;
05300 PTR←HASH(S);
05400 WHILE LENGTH(SS←VECTNAM[PTR]) ∧ ¬EQU(SS,S) DO PTR←REHASH;
05500 IF ¬LENGTH(SS)
05600 THEN BEGIN
05700 IF FREE_DATA+1>FREE_DATA_LENGTH THEN RETURN;
05800 VECTNAM[PTR]←S;
05900 VECTNUM[PTR]←FREE_DATA;
06000 FREE_DATA←FREE_DATA+1 END;
06100 ARRBLT(DATA_BASE[VECTNUM[PTR],1],V[1],3)
06200 END "SET_VECT";
06300
06400 SIMPLE MESSAGE PROCEDURE GET_TRANS(STRING S;REAL ARRAY T);
06500 BEGIN "GET_TRANS"
06600 INTEGER PTR;
06700 STRING SS;
06800 SAFE OWN REAL ARRAY E[1:6];
06900 PTR←HASH(S);
07000 WHILE LENGTH(SS←TRANSNAM[PTR]) ∧ ¬EQU(SS,S) DO PTR←REHASH;
07100 IF ¬LENGTH(SS) THEN RETURN;
07200 ARRBLT(E[1],DATA_BASE[TRANSNUM[PTR],1],6);
07300 CONSTRUCT(T,E)
07400 END "GET_TRANS";
07500
07600 SIMPLE MESSAGE PROCEDURE GET_VECT(STRING S;REAL ARRAY V);
07700 BEGIN "GET_VECT"
07800 INTEGER PTR;
07900 STRING SS;
08000 PTR←HASH(S);
08100 WHILE LENGTH(SS←VECTNAM[PTR]) ∧ ¬EQU(SS,S) DO PTR←REHASH;
08200 IF ¬LENGTH(SS) THEN RETURN;
08300 ARRBLT(V[1],DATA_BASE[VECTNUM[PTR],1],3);
08400 V[4]←1.0
08500 END "GET_VECT";
08600 ENDC
08700
08800 PROCEDURE POLY(INTEGER N);BEGIN
08900 SAFE REAL ARRAY A,LU[1:(3*MAX_SEG)+6,1:(3*MAX_SEG)+6];
09000 SAFE REAL ARRAY B[1:(3*MAX_SEG)+6];
09100 INTEGER I,J,K,P1,N3;
09200 REAL T,T2;
09300 SIMPLE PROCEDURE THREE(INTEGER I,J;REAL T);
09400 BEGIN
09500 FOR K←J STEP 1 UNTIL J+2 DO A[I+2,K]←1.0;
09600 A[I,J]←-T;
09700 A[I+1,J+1]←-(A[I+4,J+1]←2.0*(T2←T*T));
09800 A[I+4,J+2]←6.0*T2;
09900 A[I+3,J+2]←3.0*T;
10000 A[I+3,J+1]←2.0*T;
10100 A[I+3,J]←T;
10200 END;
10300
10400 SIMPLE PROCEDURE FOUR(INTEGER I,J;REAL T);
10500 BEGIN
10600 FOR K←J STEP 1 UNTIL J+3 DO A[I+2,K]←1.0;
10700 A[I,J]←-T;
10800 A[I+1,J+1]←-(A[I+4,J+1]←2.0*(T2←T*T));
10900 A[I+4,J+2]←6.0*T2;
11000 A[I+4,J+3]←12.0*T2;
11100 A[I+3,J+3]←4.0*T;
11200 A[I+3,J+2]←3.0*T;
11300 A[I+3,J+1]←2.0*T;
11400 A[I+3,J]←T;
11500 END;
11600
11700 UNDERFLOW(14);
11800 N3←N*3;
11900 T←1.0/TIM[1];
12000 T2←T*T;
12100 A[1,1]←A[1,2]←1.0;
12200 A[2,1]←3.0*T;
12300 A[2,2]←4.0*T;
12400 A[3,1]←6.0*T2;
12500 A[3,2]←12.0*T2;
12600 FOR J←1 STEP 1 UNTIL N-2 DO
12700 THREE((3*J)-1,(3*J),1.0/TIM[J+1]);
12800 A[N3-4,N3-3]←-3.0*(T←1.0/TIM[N]);
12900 A[N3-4,N3-2]←4.0*T;
13000 A[N3-3,N3-3]←6.0*(T2←T*T);
13100 A[N3-3,N3-2]←-12.0*T2;
13200 A[N3-2,N3-3]←1.0;
13300 A[N3-2,N3-2]←-1.0;
13400 FOR J←1 STEP 1 UNTIL N DO
13500 B[(J*3)-2]←POS[J]-POS[J-1];
13600 P1←N3-2;
13700 IF LOOP THEN BEGIN
13800 FOUR(N3-1,N3-1,(T←1/TIM[N-2]));
13900 FOUR(N3+2,N3+3,T);
14000 FOR J←6,7,8 DO BEGIN
14100 A[N3+5,J]←A[5,J];
14200 A[N3+6,J]←A[6,J] END;
14300 FOR J←N3-9 STEP 1 UNTIL N3-7 DO BEGIN
14400 A[N3-1,J]←A[N3-7,J];
14500 A[N3,J]←A[N3-6,J] END;
14600 B[N3+1]←B[N3-2]+B[N3-5];
14700 B[N3+4]←B[1]+B[4];
14800 P1←P1+8;
14900 END;
15000 DECOMPOSE(P1,A,LU);
15100 SOLVE(P1,LU,B,SOLN);
15200 IMPROVE(P1,A,LU,B,SOLN,T2);
15300 UNDERFLOW(0);
15400 END;
15500 PRELOAD_WITH 0.5, 0.5, 0.25, 1.0, 1.0, 2.5;
15600 SAFE REAL ARRAY OSHOOT[1:6];
15700
00100 BOOLEAN SIMPLE PROCEDURE OVERSHOOT(REAL A3,A4,DELTA,OFF;REFERENCE REAL T,D);
00200 RETURN( A4 ∧ OFF< (T←-(3*A3)/(4*A4)) <1+OFF ∧(D←(A4*T+A3)*T↑3/ABS(T))
00300 ∧ (IF D*DELTA>0 THEN (D←DELTA-D) ELSE (D←-D)));
00400
00500 IFC THROWING THENC REQUIRE "THROW.SAI" SOURCE_FILE; ENDC
00600
00700 INTEGER SIMPLE PROCEDURE KISEKI;BEGIN
00800 INTEGER P2S,N,I,J,OVER;
00900 REAL ANG,T;
01000 SIMPLE PROCEDURE BUMP_UP(INTEGER I;REAL ANG,T);
01100 BEGIN INTEGER J;
01200 OVER←OVER+1;
01300 FOR J←MAX_SEG STEP -1 UNTIL I+1 DO BEGIN
01400 POSITION[JOINT,J]←POSITION[JOINT,J-1];
01500 PERIOD[JOINT,J]←PERIOD[JOINT,J-1] END;
01600 POSITION[JOINT,I]←ANG;
01700 PERIOD[JOINT,I]←T*PERIOD[JOINT,I];
01800 IF PERIOD[JOINT,I]<1 THEN PERIOD[JOINT,I]←1;
01900 PERIOD[JOINT,I+1]←PERIOD[JOINT,I+1]-PERIOD[JOINT,I] END;
02000 P2S←PTR2-7;
02100 FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
02200 ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
02300 ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
02400 POLY(N←NS[JOINT]);
02500 OVER←0;
02600 IF OVERSHOOT(SOLN[1],SOLN[2],POS[1]-POS[0],0,T,ANG)
02700 THEN BEGIN POSITION[JOINT,1]←POSITION[JOINT,1]+ANG;OVER←1 END;
02800 IF OVERSHOOT(SOLN[J←(N-1)*3],SOLN[J+1],POS[N-1]-POS[N],-1.0,T,ANG)
02900 THEN BEGIN POSITION[JOINT,N-1]←POSITION[JOINT,N-1]+ANG;OVER←1 END;
03000 IF OVER THEN BEGIN
03100 ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
03200 POLY(NS[JOINT])END;
03300 ARRBLT(KOE[JOINT,1],SOLN[1],(NS[JOINT]*3)+4) END;
03400 FOR J←1 STEP 1 UNTIL 6 DO TH[J]←POSITION[J,1];
03500 SCHEINMAN(DIAG,TH);
03600 ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
03700 FOR J←1 STEP 1 UNTIL 6 DO TH[J]←POSITION[J,NS[J]-1];
03800 SCHEINMAN(DIAG,TH);
03900 DIAG[7]←DIAG[7] LOR '2000000;
04000 ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
04100 FOR J←1 STEP 1 UNTIL 6 DO TH[J]←POSITION[J,NS[J]];
04200 SCHEINMAN(DIAG,TH);
04300 DIAG[7]←DIAG[7] LOR '2000000;
04400 ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
04500 FOR JOINT←2 STEP 1 UNTIL 5 DO BEGIN
04600 OVER←0;
04700 FOR I←NS[JOINT]-1 STEP -1 UNTIL 2 DO
04800 IF QUADROOT(POSITION[JOINT,I-1],KOE[JOINT,(I*3)-3],
04900 KOE[JOINT,(I*3)-2],KOE[JOINT,(I*3)-1],ANG,T)
05000 THEN BUMP_UP(I,ANG,T);
05100 NS[JOINT]←NS[JOINT]+OVER;
05200 IF OVER THEN BEGIN
05300 ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
05400 ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
05500 POLY(NS[JOINT]);
05600 ARRBLT(KOE[JOINT,1],SOLN[1],(NS[JOINT]*3)+4) END END;
05700 PTR2←P2S;
05800 PACK_UP;
05900 RETURN(0) END;
06000
00100 SIMPLE MESSAGE PROCEDURE TRAJECTORY(REAL ARRAY T0,TF);
00200 BEGIN
00300 INTEGER MT,I,J,N;
00400 REAL INT,R;
00500 SAFE OWN REAL ARRAY AI,AF,DTH[1:6];
00600 INTEGER TR1,TR2;
00700 FLUSHP(150,T0);
00800 LIFTOFF(T0,AI,DEPART_ARM,TR1,T1_ARM);
00900 LIFTOFF(TF,AF,ARRIVE_ARM,TR2,T1_ARM);
01000 IF T1_ARM≤0 ∧ TR2<50 THEN TR2←50;
01100 FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←TF[I]+AF[I]-T0[I]-AI[I];
01200 N←IF T2_ARM>0 THEN T2_ARM ELSE RUNTIME(DTH);
01300 FOR I←1 STEP 1 UNTIL 6 DO BEGIN
01400 POSITION[I,0]←T0[I];
01500 POSITION[I,1]←T0[I]+AI[I];
01600 POSITION[I,2]←TF[I]+AF[I];
01700 POSITION[I,3]←TF[I];
01800 NS[I]←3;
01900 PERIOD[I,1]←TR1;
02000 PERIOD[I,2]←N;
02100 PERIOD[I,3]←TR1 END;
02200 LOOP←0;
02300 DURATION←N;
02400 IF KISEKI THEN USERERR(0,1,"OVERSHOOT");
02500 ARRTRAN(LAST_ARM,TF);
02600 ARRTRAN(LAST_PLANNED_ARM,TF);
02700 ARM_TIME←ARM_TIME+N+TR1*2;
02800 ARRTRAN(DEPART_ARM,ARRIVE_ARM);
02900 FOR I←1 STEP 1 UNTIL 3 DO ARRIVE_ARM[I]←IF I=3 THEN 3.0 ELSE 0.0;
03000 T1_ARM←T2_ARM←0;
03100 RESET_CONO;
03200 END;
03300 BOOLEAN SIMPLE PROCEDURE IS_NOT_CLEAR(SAFE REAL ARRAY TRANS,J);
03400 BEGIN SAFE OWN REAL ARRAY IP[1:4];
03500 REAL R;
03600 BOOLEAN SIMPLE PROCEDURE NOT_CLEAR(REFERENCE REAL J;SAFE REAL ARRAY V);
03700 BEGIN SAFE OWN REAL ARRAY VT[1:4];
03800 EXTERNAL SIMPLE PROCEDURE TRANSFORM(REAL ARRAY V;REFERENCE REAL T;REAL ARRAY F);
03900 TRANSFORM(VT,J,V);
04000 IF VT[3]<0.375 THEN RETURN(TRUE);
04100 IF (VT[1]-SHOLDER[1])↑2+(VT[2]-SHOLDER[2])↑2<25.0 THEN RETURN(TRUE) ELSE RETURN(FALSE);
04200 END;
04300 IP[1]←IP[3]←0.0;IP[4]←1.0;
04400 FOR R←-2.0,2.0 DO BEGIN IP[2]←R;IF NOT_CLEAR(TRANS[1,1],IP)THEN RETURN(TRUE);END;
04500 ARRTRAN(J,LAST_ARM);
04600 IF ARM_SOLVE(TRANS,J)=1 THEN RETURN (TRUE);
04700 HANDPOS(J);
04800 IP[2]←0.0;
04900 FOR R←3.0,-3.0 DO BEGIN IP[3]←R;IF NOT_CLEAR(T[SQAR(4)],IP) THEN RETURN(TRUE);END;
05000 IP[3]←-38.0;
05100 IF NOT_CLEAR(T[SQAR(3)],IP) THEN RETURN(TRUE) ELSE RETURN(FALSE);
05200 END;
05300
00100 SIMPLE MESSAGE PROCEDURE TO_ARM(REAL ARRAY T;REFERENCE INTEGER FLAG);
00200 BEGIN SAFE OWN REAL ARRAY J[1:6];
00300 SAFE OWN REAL ARRAY EXF[1:7];
00400 INTEGER K;
00500 FLAG←0;
00600 IF IS_NOT_CLEAR(T,J) THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
00700 FLAG←-1;
00800 IF ARM_EXECUTE THEN BEGIN
00900 ARM_MESSAGE[21]←'17000000;
01000 ARRBLT(ARM_MESSAGE[1],T[1,1],12);
01100 SCHEINMAN(EXF,J);
01200 ARRBLT(ARM_MESSAGE[14],EXF[1],7);
01300 RESET_CONO;
01400 ARM_MESSAGE[13]←0;
01500 ARMFN(20);
01600 ARM_EXECUTE←FALSE;
01700 UPDATE_SEG;
01800 RETURN END;
01900 FLUSHP(30,LAST_ARM);
02000 PTR3←PTR3+1;
02100 IF MERGE THEN BEGIN
02200 STACK[PTR3]←'417000000+(PTR4←PTR4+1);
02300 STACK[PTR3]↔STACK[PTR3-1];
02400 MERGE←FALSE END ELSE
02500 STACK[PTR3]←'17000000+(PTR4←PTR4+1);
02600 ARRBLT(COEFF[PTR4],T[1,1],12);
02700 COEFF[PTR4←PTR4+12]←T2_ARM;
02800 SCHEINMAN(DIAG,J);
02900 ARRBLT(COEFF[PTR4←PTR4+1],DIAG[1],7);
03000 PTR4←PTR4+6;
03100 ARRTRAN(LAST_ARM,J);
03200 ARM_TIME←ARM_TIME+T2_ARM;
03300 ARRTRAN(LAST_PLANNED_ARM,J);
03400 ARRTRAN(LAST_PLANNED_TRANS,T);
03500 ARRTRAN(LAST_TRANS,T);
03600 ARRTRAN(DEPART_ARM,ARRIVE_ARM);
03700 FOR I←1 STEP 1 UNTIL 3 DO ARRIVE_ARM[I]←IF I=3 THEN 3.0 ELSE 0.0;
03800 T1_ARM←T2_ARM←0;
03900 RESET_CONO;
04000 END;
04100
04200 SIMPLE MESSAGE PROCEDURE MOVE_ARM(REAL ARRAY T;REFERENCE INTEGER FLAG);
04300 BEGIN SAFE OWN REAL ARRAY J[1:6];
04400 IF ARM_EXECUTE THEN BEGIN TO_ARM(T,FLAG); RETURN END;
04500 FLAG←0;
04600 IF IS_NOT_CLEAR(T,J) THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
04700 FLAG←-1;
04800 TRAJECTORY(LAST_ARM,J);
04900 ARRTRAN(LAST_TRANS,T);
05000 ARRTRAN(LAST_PLANNED_TRANS,T);
05100 END;
05200
00100 SIMPLE MESSAGE PROCEDURE GO_ARM(REAL ARRAY T;REFERENCE INTEGER FLAG);
00200 BEGIN SAFE OWN REAL ARRAY TF[1:6];
00300 INTEGER K,TIME,MT,I,J,N,NNULS;
00400 REAL INT,R;
00500 SAFE OWN REAL ARRAY TH,DTH,AI,AF[1:6];
00600 SAFE OWN REAL ARRAY T1,T2[1:4,1:4];
00700 INTEGER TR1,TR2;
00800 IF ARM_EXECUTE THEN BEGIN TO_ARM(T,FLAG); RETURN END;
00900 FLAG←0;
01000 IF IS_NOT_CLEAR(T,TF) THEN BEGIN ARM_EXECUTE←FALSE;RETURN END;
01100 FLAG←-1;
01200 FLUSHP(90,LAST_ARM);
01300 PTR3←PTR3+1;
01400 IF MERGE THEN BEGIN
01500 STACK[PTR3]←'420000000+(PTR4←PTR4+1);
01600 STACK[PTR3]↔STACK[PTR3-1];
01700 MERGE←FALSE END ELSE
01800 STACK[PTR3]←'20000000+(PTR4←PTR4+1);
01900 LIFTOFF(LAST_ARM,AI,DEPART_ARM,TR1,T1_ARM);
02000 LIFTOFF(TF,AF,ARRIVE_ARM,TR2,T1_ARM);
02100 FOR I←1 STEP 1 UNTIL 6 DO TH[I]←LAST_ARM[I]+AI[I];
02200 INVERT(T1,LAST_TRANS);
02300 MOVET(T2,LAST_TRANS);
02400 FOR I←1 STEP 1 UNTIL 3 DO T2[I,4]←T2[I,4]+DEPART_ARM[I];
02500 TIMES(T1,T1,T2);
02600 ARRBLT(COEFF[PTR4],T1[1,1],12);
02700 COEFF[PTR4←PTR4+12]←T1_ARM;
02800 INVERT(T1,T);
02900 MOVET(T2,T);
03000 FOR I←1 STEP 1 UNTIL 3 DO T2[I,4]←T2[I,4]+ARRIVE_ARM[I];
03100 TIMES(T1,T1,T2);
03200 ARRBLT(COEFF[PTR4←PTR4+1],T1[1,1],12);
03300 COEFF[PTR4←PTR4+12]←T2_ARM;
03400 ARRBLT(COEFF[PTR4←PTR4+1],T[1,1],12);
03500 COEFF[PTR4←PTR4+12]←T1_ARM;
03600 SCHEINMAN(DIAG,TH);
03700 ARRBLT(COEFF[PTR4←PTR4+1],DIAG[1],7);
03800 FOR I←1 STEP 1 UNTIL 6 DO TH[I]←TF[I]+AF[I];
03900 SCHEINMAN(DIAG,TH);
04000 DIAG[7]←DIAG[7] LOR '2000000;
04100 ARRBLT(COEFF[PTR4←PTR4+7],DIAG[1],7);
04200 SCHEINMAN(DIAG,TF);
04300 DIAG[7]←DIAG[7] LOR '2000000;
04400 ARRBLT(COEFF[PTR4←PTR4+7],DIAG[1],7);
04500 PTR4←PTR4+6;
04600 ARRTRAN(LAST_ARM,TF);
04700 ARM_TIME←ARM_TIME+N+TR1+TR2;
04800 ARRTRAN(LAST_PLANNED_ARM,TF);
04900 ARRTRAN(LAST_PLANNED_TRANS,T);
05000 ARRTRAN(LAST_TRANS,T);
05100 ARRTRAN(DEPART_ARM,ARRIVE_ARM);
05200 FOR I←1 STEP 1 UNTIL 3 DO ARRIVE_ARM[I]←IF I=3 THEN 3.0 ELSE 0.0;
05300 T1_ARM←T2_ARM←0;
05400 RESET_CONO;
05500 END;
00100 SIMPLE MESSAGE PROCEDURE DRAW_ARM(REAL ARRAY PROFILE;INTEGER STAT);
00200 BEGIN
00300 SAFE OWN REAL ARRAY DTH[1:6];
00400 SAFE OWN REAL ARRAY FAS[0:6,1:6];
00500 SAFE OWN REAL ARRAY ROTS,ROTA,RS,IP,RV,CV,FVV,FV,VT,DIFF[1:4];
00600 REAL AR,RR,THP4,R,MAX;
00700 INTEGER TIME,NF,I,J,K,NT;
00800 EXTERNAL SIMPLE PROCEDURE MOVEV(REAL ARRAY R;REFERENCE REAL A);
00900 IF ARM_EXECUTE THEN BEGIN STAT←4;ARM_EXECUTE←FALSE;RETURN END;
01000 ARRTRAN(TRANS,LAST_TRANS);
01100 CVV(IP,LAST_TRANS,4);
01200 ARRTRAN(FAS,FREE_ARM);
01300 MOVEV(DIFF,PROFILE[1,1]);
01400 MOVEV(RV,PROFILE[2,1]);
01500 IF(AR←IF MAGNITUDE(RV) THEN PROFILE[3,1] ELSE 0) THEN UNIT(RV,RV);
01600 MOVEV(ROTA,PROFILE[5,1]);
01700 IF(RR←IF MAGNITUDE(ROTA) THEN PROFILE[3,2] ELSE 0)
01800 THEN BEGIN UNIT(ROTA,ROTA);
01900 MOVEV(ROTS,PROFILE[4,1]);
02000 DIFFERENCE(IP,IP,ROTS) END;
02100 FVV[1]←FV[1]←FORCE_ARM[1];
02200 FVV[2]←FV[2]←FORCE_ARM[2];
02300 FVV[3]←FV[3]←FORCE_ARM[3];
02400 FVV[4]←FV[4]←1.0;
02500 FLUSH(0,LAST_ARM);
02600 SCHEINMAN(DIAG,LAST_ARM);
02700 ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
02800 FRST_OPEN←TRUE;
02900 R←0;
03000 IF PROFILE[0,3] THEN LOOP←PROFILE[0,3]-1;
03100 IF LOOP THEN NT←8 ELSE BEGIN
03200 NT←4;
03300 R←MAGNITUDE(DIFF);
03400 IF R>15.0 THEN NT←(R/5)+0.5;
03500 NT←IF (MAX←AR/60.0)>NT THEN MAX ELSE NT;
03600 NT←IF (MAX←RR/60.0)>NT THEN MAX ELSE NT END;
03700 IF TYP_HAND THEN OUTSTR(CVS(NT)&" PART TRAJECTORY"&CRLF);
03800 IF NT>8 THEN BEGIN STAT←3;RETURN END;
03900 FOR J←1 STEP 1 UNTIL 6 DO TH[I]←POSITION[J,0]←LAST_ARM[J];
04000 FOR I←1 STEP 1 UNTIL NT DO
04100 BEGIN
04200 R←IF I=1
04300 THEN 1/(3*(NT-2))
04400 ELSE (I-1)/(NT-2);
04500 R←IF I=NT-1
04600 THEN R-1/(3*(NT-2))
04700 ELSE IF I=NT
04800 THEN 1.0
04900 ELSE R;
05000 SCALE(VT,DIFF,R);
05100 PLUS(VT,VT,IP);
05200 IF RR THEN BEGIN ROTATE(RS,ROTS,ROTA,R*RR);
05300 PLUS(VT,VT,RS)END;
05400 REDUCE(VT);
05500 CVC(TRANS,4,VT);
05600 IF AR THEN FOR J←1 STEP 1 UNTIL 3 DO BEGIN
05700 CVV(CV,LAST_TRANS,J);
05800 REVOLVE(CV,RV,R*AR);
05900 CVC(TRANS,J,CV) END;
06000 IF (STAT←ARM_SOLVE(TRANS,TH)) THEN RETURN;
06100 FOR J←1 STEP 1 UNTIL 6 DO BEGIN
06200 POSITION[J,I]←TH[J];
06300 DTH[I]←POSITION[J,I]-POSITION[J,I-1] END;
06400 IF (K←RUNTIME(DTH))*NT > PROFILE[0,2] THEN PROFILE[0,2]←K*NT;
06500 IF RR THEN BEGIN
06600 FOR K←1 STEP 1 UNTIL FAS[0,1] DO
06700 BEGIN
06800 MOVEV(VT,FAS[K,1]);
06900 VT[4]←1.0;
07000 IF MAGNITUDE(VT)>0.0 THEN
07100 BEGIN
07200 REVOLVE(VT,ROTA,R*RR);
07300 REDUCE(VT);
07400 FOR J←1 STEP 1 UNTIL 3 DO FREE_ARM[K,J]←VT[J] END END;
07500 MOVEV(FV,FVV[1]);
07600 REVOLVE(FV,ROTA,R*RR);
07700 REDUCE(FV);
07800 ARRBLT(FORCE_ARM[1],FV[1],3) END;
07900 SCHEINMAN(DIAG,TH);
08000 ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
08100 END;
08200 TIME←(PROFILE[0,2]/NT)+0.5;
08300 FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
08400 NS[JOINT]←NT;
08500 FOR J←1 STEP 1 UNTIL NT DO PERIOD[JOINT,J]←TIME;
08600 ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
08700 ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
08800 POLY(NS[JOINT]);
08900 ARRBLT(KOE[JOINT,1],SOLN[1],(NS[JOINT]*3)+6) END;
09000 PTR2←486;
09100 PACK_UP;
09200 DURATION←TIME;
09300 ARRTRAN(LAST_ARM,TH);
09400 ARRTRAN(LAST_TRANS,TRANS);
09500 ARRTRAN(LAST_PLANNED_ARM,TH);
09600 ARRTRAN(LAST_PLANNED_TRANS,TRANS);
09700 ARM_TIME←ARM_TIME+TIME*((NT-2)*LOOP+2);
09800 STAT←0;
09900 RESET_CONO;
10000 FRST_OPEN←FALSE;
10100 END;
00100 SIMPLE MESSAGE PROCEDURE START_TRAJECTORY(STRING FILE;INTEGER START);
00200 BEGIN
00300 STRING S;
00400 LABEL FIND,NONE,FOUND;
00500 REAL R;
00600 FRST_OPEN←TRUE;
00700 ARM_TIME←0;
00800 IF ¬START THEN ARRTRAN(LAST_ARM,ARM_VECTOR);
00900 ARRTRAN(LAST_PLANNED_ARM,LAST_ARM);
01000 HANDPOS(LAST_ARM);
01100 ARRBLT(LAST_TRANS[1,1],T[SQAR(6)],16);
01200 ARRTRAN(LAST_PLANNED_TRANS,LAST_TRANS);
01300 FOR I←1 STEP 1 UNTIL 3 DO ARRIVE_ARM[I]←DEPART_ARM[I]←IF I=3 THEN 3.0 ELSE 0.0;
01400 RESET_CONO;
01500 IF FAST THEN BEGIN
01600 FIND: IF NEXT_BAND>'37 THEN GO TO NONE;
01700 FOR BAND←0 STEP 1 UNTIL NEXT_BAND-1 DO IF EQU(FILE,BANDS[BAND])THEN GO TO FOUND;
01800 TRACK[BAND]←CALL('100000000000 LOR BAND,"UFBGET");
01900 IF ¬_SKIP_ THEN GO TO NONE;
02000 NEXT_BAND←BAND+1;
02100 BANDS[BAND]←FILE;
02200 FOUND: GOODIE[3]←0;
02300 BAND_NUMBER←TRACK[BAND];
02400 END ELSE NONE:BEGIN
02500 FAST←FALSE;
02600 OPEN('16,"DSK",'17,0,0,120,BREAK,EOF);
02700 ENTER('16,FILE&".TRJ",BREAK);
02800 END;
02900 COEFF[0]←0; ARRBLT(COEFF[1],COEFF[0],'1037);
03000 PTR4←PTR3←0;
03100 PTR2←512;
03200 END;
03300
03400 SIMPLE MESSAGE PROCEDURE CLOSE_TRAJECTORY;
03500 FLUSH(1,TH);
03600
03700 SIMPLE MESSAGE PROCEDURE ARM_POSITION;
03800 BEGIN
03900 ARMPOS;
04000 UPDATE_SEG;
04100 END;
04200
00100 SIMPLE MESSAGE PROCEDURE OPEN_HAND(REAL OPENING);
00200 IF ARM_EXECUTE
00300 THEN BEGIN ARM_MESSAGE[1]←'1000000 LOR (OPENING LSH -18);
00400 HANDFN;
00500 ARM_EXECUTE←FALSE;
00600 UPDATE_SEG END
00700 ELSE BEGIN
00800 FLUSHP(150,LAST_ARM);
00900 STACK[PTR3←PTR3+1]←'1000000 LOR (OPENING LSH -18) END;
01000
01100 SIMPLE MESSAGE PROCEDURE WOBBLE_HAND(REAL WOBBLEING);
01200 IF ARM_EXECUTE
01300 THEN BEGIN ARM_MESSAGE[1]←'14000000 LOR (WOBBLEING LSH -18);
01400 HANDFN;
01500 ARM_EXECUTE←FALSE;
01600 UPDATE_SEG END
01700 ELSE BEGIN FLUSHP(1,LAST_ARM);STACK[PTR3←PTR3+1]←'14000000 LOR (WOBBLEING LSH -18) END;
01800
01900 SIMPLE MESSAGE PROCEDURE PLACE_ARM;
02000 BEGIN SAFE OWN REAL ARRAY DIR[1:4],DTH[1:6];
02100 INTEGER I;
02200 FOR I←1 STEP 1 UNTIL 3 DO DIR[I]←IF I=3 THEN -0.03 ELSE 0;
02300 DIR[4]←1.0;
02400 INCREMENT(DTH,DIR,FALSE);
02500 IF ARM_EXECUTE
02600 THEN BEGIN ARM_MESSAGE[7]←'4000000;
02700 ARRBLT(ARM_MESSAGE[1],DTH[1],6);
02800 ARMFN(6);
02900 ARM_EXECUTE←FALSE;
03000 UPDATE_SEG END
03100 ELSE BEGIN FLUSHP(7,LAST_ARM);
03200 STACK[PTR3←PTR3+1]←'4000000+(PTR4←PTR4+1);
03300 ARRBLT(COEFF[PTR4],DTH[1],6);
03400 PTR4←PTR4+5 END;
03500 END;
03600
03700 SIMPLE MESSAGE PROCEDURE SEARCH_ARM(REAL SIZE;REAL ARRAY N,F);
03800 BEGIN SAFE OWN REAL ARRAY SEC[1:4];
03900 IF ARM_EXECUTE THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
04000 CROSS(SEC,N,F);
04100 UNIT(F,F);
04200 UNIT(SEC,SEC);
04300 SCALE(F,F,SIZE);
04400 SCALE(SEC,SEC,SIZE);
04500 REDUCE(F);
04600 REDUCE(SEC);
04700 FLUSHP(40,LAST_ARM);
04800 STACK[PTR3←PTR3+1]←'15000000+(PTR4←PTR4+1);
04900 ARRBLT(COEFF[PTR4],F[1],3);
05000 ARRBLT(COEFF[PTR4←PTR4+3],SEC[1],3);
05100 ARRBLT(COEFF[PTR4←PTR4+3],LAST_PLANNED_TRANS[1,1],12);
05200 ARRBLT(COEFF[PTR4←PTR4+12],LAST_PLANNED_ARM[1],6);
05300 PTR4←PTR4+6;
05400 END;
05500
05600 SIMPLE MESSAGE PROCEDURE PARK_ARM;
05700 BEGIN INTEGER K;
05800 IF ¬ARM_EXECUTE THEN
05900 BEGIN
06000 ARRIVE_ARM[3]←0.0;
06100 GO_ARM(PARK_TRANS,K);
06200 END ELSE TO_ARM(PARK_TRANS,K);
06300 END;
06400
06500 SIMPLE MESSAGE PROCEDURE WAIT_ARM(STRING S);
06600 IF ¬ARM_EXECUTE THEN BEGIN
06700 FLUSHP(16,LAST_ARM);
06800 STACK[PTR3←PTR3+1]←'3000000+(PTR4←PTR4+1);
06900 PTR4←PTR4+PSTRING(COEFF[PTR4],S[1 FOR 74]);
07000 END ELSE ARM_EXECUTE←FALSE;
07100
07200 SIMPLE MESSAGE PROCEDURE CLOSE_HAND(REAL DIST);
07300 IF ARM_EXECUTE
07400 THEN BEGIN ARM_MESSAGE[1]←'2000000 LOR (DIST LSH -18);
07500 HANDFN;
07600 ARM_EXECUTE←FALSE;
07700 UPDATE_SEG END
07800 ELSE BEGIN FLUSHP(150,LAST_ARM);STACK[PTR3←PTR3+1]←'2000000 LOR (DIST LSH -18)END;
07900
00100 SIMPLE MESSAGE PROCEDURE CHANGE_ARM(REAL ARRAY DIR;REAL DIST;
00200 REAL ARRAY AXIS;REAL DEG;INTEGER TIME;REFERENCE INTEGER FLAG);
00300 BEGIN INTEGER J,N;
00400 SAFE OWN REAL ARRAY AV[1:6];
00500 SAFE OWN REAL ARRAY EXF[1:7];
00600 SAFE OWN REAL ARRAY DT,TT[1:4,1:4];
00700 SAFE OWN REAL ARRAY VT[1:4];
00800 IF ARM_EXECUTE
00900 THEN ARRBLT(TT[1,1],ARM_LINK[6,1,1],16)
01000 ELSE ARRTRAN(TT,LAST_TRANS);
01100 SCALE(VT,DIR,DIST);
01200 REDUCE(VT);
01300 ARRTRAN(DT,TT);
01400 FOR J←1 STEP 1 UNTIL 3 DO DT[J,4]←DT[J,4]+VT[J];
01500 IF DEG ∧ MAGNITUDE(AXIS) THEN BEGIN
01600 UNIT(AXIS,AXIS);
01700 FOR I←1 STEP 1 UNTIL 3 DO BEGIN
01800 CVV(VT,DT,I);
01900 REVOLVE(VT,AXIS,DEG);
02000 CVC(DT,I,VT) END;
02100 END;
02200 INVERT(TT,TT);
02300 TIMES(TT,TT,DT);
02400 IF ARM_EXECUTE THEN ARRTRAN(AV,ARM_VECTOR) ELSE ARRTRAN(AV,LAST_ARM);
02500 FLAG←¬ARM_SOLVE(DT,AV);
02600 IF ¬FLAG THEN BEGIN ARM_EXECUTE←FALSE;RETURN END;
02700 IF ARM_EXECUTE
02800 THEN BEGIN ARM_MESSAGE[21]←'5000000;
02900 ARRBLT(ARM_MESSAGE[1],TT[1,1],12) END
03000 ELSE BEGIN ARRTRAN(LAST_ARM,AV);
03100 FLUSHP(70,AV);
03200 PTR3←PTR3+1;
03300 IF MERGE THEN BEGIN
03400 STACK[PTR3]←'405000000+(PTR4←PTR4+1);
03500 STACK[PTR3]↔STACK[PTR3-1];
03600 MERGE←FALSE END ELSE
03700 STACK[PTR3]←'5000000+(PTR4←PTR4+1);
03800 ARRBLT(COEFF[PTR4],TT[1,1],12) END;
03900 SCHEINMAN(EXF,AV);
04000 IF ARM_EXECUTE
04100 THEN ARRBLT(ARM_MESSAGE[13],EXF[1],7)
04200 ELSE BEGIN ARRBLT(COEFF[PTR4←PTR4+12],EXF[1],7);
04300 PTR4←PTR4+6 END;
04400 RESET_CONO;
04500 IF ARM_EXECUTE
04600 THEN BEGIN ARM_MESSAGE[20]←TIME;
04700 ARMFN(20);
04800 ARM_EXECUTE←FALSE;
04900 UPDATE_SEG END
05000 ELSE BEGIN COEFF[PTR4←PTR4+1]←TIME;
05100 ARM_TIME←ARM_TIME+TIME;
05200 ARRTRAN(LAST_TRANS,DT) END;
05300 END;
05400
00100 SIMPLE MESSAGE PROCEDURE CENTER_HAND(REAL DIST);
00200 BEGIN INTEGER J,N;
00300 IF ARM_EXECUTE
00400 THEN BEGIN ARM_MESSAGE[2]←'12000000;
00500 ARRBLT(ARM_MESSAGE[1],DIST,1);
00600 ARMFN(1);
00700 ARM_EXECUTE←FALSE;
00800 UPDATE_SEG END
00900 ELSE BEGIN FLUSHP(4,LAST_ARM);
01000 STACK[PTR3←PTR3+1]←'12000000+(PTR4←PTR4+1);
01100 ARRBLT(COEFF[PTR4],DIST,1) END;
01200 END;
01300
01400 SIMPLE MESSAGE PROCEDURE SET_ARM(STRING ST;REAL ARRAY V,WRT);
01500 BEGIN
01600 SAFE OWN REAL ARRAY T[1:4,1:4];
01700 INVERT(T,WRT);
01800 TIMES(T,V,T);
01900 IF ARM_EXECUTE THEN BEGIN
02000 ARM_MESSAGE[14]←'13000000;
02100 ARM_MESSAGE[1]←CVSIX(ST);
02200 ARRBLT(ARM_MESSAGE[2],T[1,1],12);
02300 ARMFN(13);
02400 ARM_EXECUTE←FALSE;
02500 UPDATE_SEG END
02600 ELSE BEGIN
02700 FLUSHP(24,LAST_ARM);
02800 STACK[PTR3←PTR3+1]←'13000000+(PTR4←PTR4+1);
02900 COEFF[PTR4]←CVSIX(ST);
03000 ARRBLT(COEFF[PTR4←PTR4+1],T[1,1],12);
03100 PTR4←PTR4+11 END;
03200 END"SET_ARM";
03300
03400 SIMPLE MESSAGE PROCEDURE MOVING(STRING ST;REAL ARRAY V);
03500 BEGIN
03600 IF ARM_EXECUTE THEN BEGIN
03700 ARM_MESSAGE[5]←'24000000;
03800 ARM_MESSAGE[1]←CVSIX(ST);
03900 ARRBLT(ARM_MESSAGE[2],V[1],3);
04000 ARMFN(3);
04100 ARM_EXECUTE←FALSE;
04200 UPDATE_SEG END
04300 ELSE BEGIN
04400 FLUSHP(4,LAST_ARM);
04500 STACK[PTR3←PTR3+1]←'24000000+(PTR4←PTR4+1);
04600 COEFF[PTR4]←CVSIX(ST);
04700 ARRBLT(COEFF[PTR4←PTR4+1],V[1],3);
04800 PTR4←PTR4+2 END;
04900 END"MOVING";
05000
00100 SIMPLE MESSAGE PROCEDURE DRIVE_ARM(INTEGER JOINT;REAL DEG;INTEGER TIME;REFERENCE INTEGER FLAG);
00200 BEGIN INTEGER I,J,N;
00300 REAL R;
00400 SAFE OWN REAL ARRAY EXF[1:7];
00500 SAFE OWN REAL ARRAY LA[1:6];
00600 FLAG←FALSE;
00700 IF ARM_EXECUTE THEN ARRBLT(LA[1],ARM_VECTOR[1],6) ELSE ARRTRAN(LA,LAST_ARM);
00800 R←LA[JOINT]+DEG;
00900 IF (STOP[JOINT,1]-R)*(R-STOP[JOINT,2])<1 THEN BEGIN ARM_EXECUTE←FALSE;RETURN END;
01000 FLAG←TRUE;
01100 LA[JOINT]←R;
01200 SCHEINMAN(EXF,LA);
01300 IF ARM_EXECUTE
01400 THEN BEGIN ARM_MESSAGE[15]←'23000000;
01500 ARM_MESSAGE[1]←0;ARRBLT(ARM_MESSAGE[2],ARM_MESSAGE[1],5);
01600 ARRBLT(ARM_MESSAGE[JOINT],DEG,1) END
01700 ELSE BEGIN ARRBLT(LAST_TRANS[1,1],T[85],16);
01800 FLUSHP(50,LAST_ARM);
01900 PTR3←PTR3+1;
02000 IF MERGE THEN BEGIN
02100 STACK[PTR3]←'423000000+(PTR4←PTR4+1);
02200 STACK[PTR3]↔STACK[PTR3-1];
02300 MERGE←FALSE END ELSE
02400 STACK[PTR3]←'23000000+(PTR4←PTR4+1);
02500 COEFF[PTR4]←0;
02600 ARRBLT(COEFF[PTR4+1],COEFF[PTR4],5);
02700 ARRBLT(COEFF[PTR4+JOINT-1],DEG,1) END;
02800 IF ARM_EXECUTE
02900 THEN BEGIN ARRBLT(ARM_MESSAGE[7],EXF[1],7);
03000 ARM_MESSAGE[13]←GO_WORD[JOINT]+2;
03100 IF FREE_ARM[0,1] THEN FREE_JOINT(ARM_MESSAGE[13]);
03200 ARM_MESSAGE[14]←TIME;
03300 ARMFN(14);
03400 ARM_EXECUTE←FALSE;
03500 UPDATE_SEG END
03600 ELSE BEGIN ARRBLT(COEFF[PTR4←PTR4+6],EXF[1],7);
03700 PTR4←PTR4+6;
03800 IFC BLUE THENC
03900 COEFF[PTR4]←GO_WORD[JOINT]+4;
04000 ELSEC
04100 COEFF[PTR4]←GO_WORD[JOINT]+2;
04200 ENDC
04300 IF NNUL THEN COEFF[PTR4]←COEFF[PTR4]+'1000000;
04400 IF FREE_ARM[0,1] THEN FREE_JOINT(COEFF[PTR4]);
04500 COEFF[PTR4←PTR4+1]←TIME;
04600 ARM_TIME←ARM_TIME+TIME;
04700 ARRTRAN(LAST_ARM,LA) END;
04800 RESET_CONO;
04900 END;
05000
05100 SIMPLE MESSAGE PROCEDURE STOP_ARM(REAL ARRAY F,M);
05200 BEGIN SAFE OWN REAL ARRAY TQ,XF[1:6];
05300 INTEGER I;
05400 REAL FAC,R,MAX;
05500 HANDPOS(LAST_ARM);
05600 REDUCE(F);
05700 ARRBLT(XF[1],F[1],3);
05800 REDUCE(M);
05900 ARRBLT(XF[4],M[1],3);
06000 FORCE(TQ,XF);
06100 MAX←0.0;
06200 FAC←1.5;
06300 FOR I←1 STEP 1 UNTIL 6 DO IF (R←ABS(TQ[I]/F0[I]))>MAX THEN BEGIN
06400 MAX←R;
06500 BFJ←I END;
06600 R←FAC/MAX;
06700 IF R>1.0 THEN FOR I←1 STEP 1 UNTIL 6 DO TQ[I]←TQ[I]*R;
06800 FLUSHP(7,LAST_ARM);
06900 STACK[PTR3←PTR3+1]←'7000000+(PTR4←PTR4+1);
07000 ARRBLT(COEFF[PTR4],TQ[1],6);
07100 PTR4←PTR4+5;
07200 END;
07300
07400 SIMPLE MESSAGE PROCEDURE NO_NULL;NNUL←-1;
07500
00100 SIMPLE MESSAGE PROCEDURE TALK_ARM;INTERP←TRUE;
00200
00300 SIMPLE MESSAGE PROCEDURE DO_IT(INTEGER PPPN;STRING FILE);
00400 BEGIN INTEGER I;
00500 ARM_STATUS←0;
00600 ARM_EXECUTE←FALSE;
00700 FOR I←0 STEP 1 UNTIL NEXT_BAND-1 DO IF EQU(FILE,BANDS[I])THEN DONE;
00800 DOIT(PPPN,TRACK[I],CVSIX(FILE));
00900 IF ARM_STATUS='50 ∧TRACK[I] THEN BANDS[I]←NULL;
01000 UPDATE_SEG;
01100 END;
01200
01300 SIMPLE MESSAGE PROCEDURE MERGE_ARM;MERGE←TRUE;
01400
01500 SIMPLE MESSAGE PROCEDURE DO_PROCEED(INTEGER SKIP);
01600 BEGIN
01700 ARM_EXECUTE←FALSE;
01800 IF ¬ARM_WAIT THEN RETURN;
01900 ARMPROCEED(SKIP);
02000 UPDATE_SEG;
02100 END;
02200
02300 SIMPLE MESSAGE PROCEDURE ARM_CONO(REAL ARRAY APPROACH,OBJECT;INTEGER DEPROACH_TIME,MID_TIME);
02400 BEGIN ARRTRAN(ARRIVE_ARM,APPROACH);
02500 OBJECT_MASS←OBJECT[4];
02600 OBJECT_KXX←OBJECT[1];
02700 OBJECT_KYY←OBJECT[2];
02800 OBJECT_KZZ←OBJECT[3];
02900 T2_ARM←MID_TIME;
03000 T1_ARM←DEPROACH_TIME;
03100 END;
03200
03300 SIMPLE MESSAGE PROCEDURE SET_TOUCH(BOOLEAN STOP_ON_TOUCH);
03400 IF ¬ARM_EXECUTE THEN BEGIN FLUSHP(1,LAST_ARM);
03500 STACK[PTR3←PTR3+1]←'6000000;
03600 IF STOP_ON_TOUCH THEN STACK[PTR3]←STACK[PTR3] LOR '777777;
03700 END ELSE ARM_EXECUTE←FALSE;
03800
03900 SIMPLE MESSAGE PROCEDURE ARM_SKIPE(INTEGER I);
04000 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'101000000 LOR I ELSE ARM_EXECUTE←FALSE;
04100
04200 SIMPLE MESSAGE PROCEDURE ARM_SKIPN(INTEGER I);
04300 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'103000000 LOR I ELSE ARM_EXECUTE←FALSE;
04400
04500 SIMPLE MESSAGE PROCEDURE ARM_SKIPS(INTEGER I);
04600 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'104000000 LOR I ELSE ARM_EXECUTE←FALSE;
04700
04800 SIMPLE MESSAGE PROCEDURE ARM_JMP(INTEGER I);
04900 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'102000000 LOR ('777777 LAND I) ELSE ARM_EXECUTE←FALSE;
05000
05100 SIMPLE MESSAGE PROCEDURE ARM_AOJ(INTEGER I);
05200 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'16000000 LOR ('777777 LAND I) ELSE ARM_EXECUTE←FALSE;
05300
05400 SIMPLE MESSAGE PROCEDURE ARM_SOJG(STRING S;INTEGER I);
05500 IF ARM_EXECUTE THEN BEGIN
05600 ARM_EXECUTE←FALSE;;
05700 RETURN END ELSE BEGIN
05800 FLUSHP(3,LAST_ARM);
05900 STACK[PTR3←PTR3+1]←'26000000+(PTR4←PTR4+1);
06000 COEFF[PTR4]←CVSIX(S);
06100 COEFF[PTR4←PTR4+1]←I END;
00100 SIMPLE MESSAGE PROCEDURE ARM_ASSIGN(STRING S;INTEGER VAL);
00200 IF ARM_EXECUTE THEN BEGIN
00300 ARM_MESSAGE[3]←'25000000;
00400 ARM_MESSAGE[1]←CVSIX(S);
00500 ARM_MESSAGE[2]←VAL;
00600 ARMFN(2);
00700 UPDATE_SEG END ELSE
00800 BEGIN
00900 FLUSHP(3,LAST_ARM);
01000 STACK[PTR3←PTR3+1]←'25000000+(PTR4←PTR4+1);
01100 COEFF[PTR4]←CVSIX(S);
01200 COEFF[PTR4←PTR4+1]←VAL END;
01300
01400 SIMPLE MESSAGE PROCEDURE ARM_SAVE(STRING S);
01500 BEGIN
01600 SAFE OWN REAL ARRAY T[1:4,1:4];
01700 INVERT(T,LAST_PLANNED_TRANS);
01800 IF ARM_EXECUTE THEN BEGIN ARM_MESSAGE[13]←'10000000;
01900 ARM_MESSAGE[1]←CVSIX(S) END ELSE
02000 BEGIN FLUSHP(30,LAST_ARM);STACK[PTR3←PTR3+1]←'10000000+(PTR4←PTR4+1);
02100 COEFF[PTR4]←CVSIX(S) END;
02200 IF ARM_EXECUTE THEN ARRBLT(ARM_MESSAGE[2],T[1,1],12)
02300 ELSE ARRBLT(COEFF[PTR4←PTR4+1],T[1,1],12);
02400 IF ARM_EXECUTE THEN BEGIN ARMFN(13);
02500 ARM_EXECUTE←FALSE;
02600 UPDATE_SEG END ELSE PTR4←PTR4+12;
02700 END;
02800
02900 SIMPLE MESSAGE PROCEDURE ARM_RESTORE(STRING S;INTEGER FINISH);
03000 BEGIN
03100 LABEL L1;
03200 INTEGER I,J;
03300 SAFE OWN REAL ARRAY T[1:4,1:4];
03400 IF ARM_EXECUTE THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
03500 FOR I←PTR3 STEP -1 UNTIL 1 DO BEGIN
03600 J←STACK[I] LAND '77000000;
03700 IF '17000000 ≤ J ≤'21000000 THEN BEGIN
03800 FOR J←PTR3 STEP -1 UNTIL I DO BEGIN
03900 STACK[J+1]←STACK[J];
04000 END;
04100 PTR3←PTR3+1;
04200 GO TO L1
04300 END;
04400 END;
04500 RETURN;
04600 L1: STACK[I]←'11000000+(PTR4←PTR4+1);
04700 COEFF[PTR4]←CVSIX(S);
04800 COEFF[PTR4←PTR4+1]←FINISH;
04900 ARRBLT(COEFF[PTR4←PTR4+1],LAST_PLANNED_TRANS[1,1],12);
05000 ARRBLT(COEFF[PTR4←PTR4+12],LAST_PLANNED_ARM[1],6);
05100 PTR4←PTR4+6;
05200 END;
05300
05400
05500 SIMPLE MESSAGE PROCEDURE SCREW(REAL VELOCITY);
05600 BEGIN INTEGER I;
05700 IF ARM_EXECUTE THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
05800 STACK[PTR3←PTR3+1]←'22000000 LOR ((I←VELOCITY*1.0) LAND '777777) END;
05900